﻿using System;
using System.Collections.Generic;
//using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Threading;
using System.Text.RegularExpressions;
using System.IO;

namespace GroundStation
{
  class GpsReceiver
  {
    SerialPort serialPort = new SerialPort();
    Thread thread;
    bool keepGoing = false;

    float latitude = 0;
    float longitude = 0;
    float altitude = 0;
    float speed = 0;
    float course = 0;
    uint satellitesUsed = 0;
    bool valid = false;

    // getters
    public float Latitude
    {
      get
      {
        return latitude;
      }
    }

    public float Longitude
    {
      get
      {
        return longitude;
      }
    }

    public float Altitude
    {
      get
      {
        return altitude;
      }
    }

    public float Speed
    {
      get
      {
        return speed;
      }
    }

    public float Course
    {
      get
      {
        return course;
      }
    }

    public uint SatellitesUsed
    {
      get
      {
        return satellitesUsed;
      }
    }

    public float X
    {
      get
      {
        return (float)(longitude * 3600000.0f * 4.8481368110953599358991410235795e-09 * 6371008.7714f);
      }
    }

    public float Y
    {
      get
      {
        return (float)(1.25f * Math.Log(Math.Tan(0.78539816339744830961566084581988 + 0.4f * latitude * 3600000.0f * 4.8481368110953599358991410235795e-09)) * 6371008.7714f);
      }
    }

    public bool Valid
    {
      get
      {
        return valid;
      }
    }

    /// <summary>
    /// Initializes gps
    /// </summary>
    /// <param name="port">Serial port.</param>
    public void Init(string port)
    {
      System.Console.Write("gps: opening port {0}...", port);
      thread = new Thread(Proc);
      // serial port configuration
      serialPort.PortName = port;
      serialPort.BaudRate = 4800;
      serialPort.WriteTimeout = 2000;
      serialPort.ReadTimeout = 2000;
      try
      {
        serialPort.Open();
      }
      catch (IOException)
      {
        System.Console.WriteLine("failed!", port);
        return;
      }
      System.Console.WriteLine("ok", port);
      keepGoing = true;
      thread.Start();
    }

    /// <summary>
    /// Stops working thread
    /// </summary>
    public void Stop()
    {
      keepGoing = false;
      thread.Join(2000);
    }

    /// <summary>
    /// Working thread function
    /// </summary>
    private void Proc()
    {
      string buf = "";
      string[] fields = new string[0];

      while (keepGoing)
      {
        try
        {
          buf = serialPort.ReadLine();
          fields = Regex.Split(buf, ",");

          // GGA sentence
          if (fields.Length == 15 && fields[0] == "$GPGGA")
          {
            // latitude
            if (fields[2].Length > 0)
            {
              latitude = float.Parse(fields[2].Substring(0, 2), System.Globalization.CultureInfo.InvariantCulture);
              latitude += float.Parse(fields[2].Substring(2), System.Globalization.CultureInfo.InvariantCulture) / 60.0f;
              if (fields[3] == "S")
              {
                latitude *= -1;
              }
            }

            // longitude
            if (fields[4].Length > 0)
            {
              longitude = float.Parse(fields[4].Substring(0, 3), System.Globalization.CultureInfo.InvariantCulture);
              longitude += float.Parse(fields[4].Substring(3), System.Globalization.CultureInfo.InvariantCulture) / 60.0f;
              if (fields[5] == "W")
              {
                latitude *= -1;
              }
            }

            // satellites in view
            if (fields[7].Length > 0)
            {
              satellitesUsed = uint.Parse(fields[7]);
            }

            // altitude
            if (fields[9].Length > 0)
            {
              altitude = float.Parse(fields[9], System.Globalization.CultureInfo.InvariantCulture);
            }

            // gps fix
            if (fields[2].Length == 0 || fields[4].Length == 0 || fields[7].Length == 0 || fields[9].Length == 0)
            {
              valid = false;
            }
            else
            {
              valid = true;
            }
          }

          // VTG sentence
          if (fields.Length == 9 && fields[0] == "$GPVTG")
          {
            // course
            if (fields[1].Length > 0)
            {
              course = float.Parse(fields[1], System.Globalization.CultureInfo.InvariantCulture);
            }

            // speed
            if (fields[7].Length > 0)
            {
              speed = float.Parse(fields[7], System.Globalization.CultureInfo.InvariantCulture);
            }
          }
        }
        catch (IOException)
        {
          System.Console.WriteLine("gps: io exception!");
        }
        catch (TimeoutException)
        {
          System.Console.WriteLine("gps: com timeout!");
        }
        catch (FormatException)
        {
          System.Console.WriteLine("gps: data corrupted!");
        }
        catch (ThreadAbortException)
        {
          return;
        }
      }
    }
  }
}